#ifndef CELLDECOMPASTAR_H
#define CELLDECOMPASTAR_H

/********** Library Includes **************************************************/

#include <opencv/cv.h>

/********** Local Includes ****************************************************/

#include "graphstructures.h"

/********** Forward Class Declarations ****************************************/

class AStarHeuristic;

/********** Classes ***********************************************************/

class CellDecompAStar {
public:
	explicit CellDecompAStar(const cv::Mat &grid, AStarHeuristic *heuristic);
	
	std::vector<cv::Point> plan(cv::Point start, cv::Point goal);
	
private:
	// TODO These should to be moved out
	
	bool isValid(const cv::Point &) const;
	
	/**
	 * Converts a vector of Node pointers representing the path to travel to a
	 * vector of points to drive.
	 *
	 * The point vector is "compressed" such that only points requiring a turn
	 * are representing.  For example, if the Node pointer vector contains Nodes
	 * at (3, 3), (4, 3), (5, 3), and (5, 4), the resulting Point vector will
	 * contain (3, 3), (5, 3), and (5, 4).
	 */
	std::vector<cv::Point> toPoints(const std::vector<Node *> &path) const;
	
	/**
	 * Returns the unit vector in the direction (two - one).
	 */
	std::vector<double> unit(cv::Point one, cv::Point two) const;
	
	typedef std::vector<Node> NodeVector;
	typedef std::vector<NodeVector> NodeMatrix;
	NodeMatrix m_nodes;
	
	AStarHeuristic *m_heuristic;
};

#endif